/* rosie.c
 *
 * Code for my H-Bridge motor controller board. A PID control algorithm
 * takes input from rotary quadrature encoders (PD5, PD6) and outputs
 * direction (PB0, PB1) and a PWM (PB2) duty cycle.
 *
 * John Boiles
 * 10 November 2008
 */

#include <inttypes.h>
#include <avr/io.h>
#include <avr/interrupt.h>
//#include <avr/sleep.h>
#include "pid.h"
//#include "stdint.h"
#include "pid.c"
#include "uart.c"
#include "encfifo.c"

//Set COM0A1:0 = 2 for non-inverted pwm and 3 for inverted pwm
//Use the following line for PhaseCorrectPWM
#  define TIMER0_PWM_INIT _BV(WGM00) | _BV(COM0A1)
//The clocksource determines the timer's frequency
//CS = 1 ClkIO = Clk
//CS = 2 ClkIO = Clk / 8
//CS = 3 ClkIO = Clk / 64
//CS = 4 ClkIO = Clk / 256
//CS = 5 ClkIO = Clk / 1024//CS = 6,7 External Clock
# define TIMER0_CLOCKSOURCE _BV(CS02) //| _BV(CS00)
# define TIMER1_CLOCKSOURCE _BV(CS11) //Clock / 8

/* The K_P, K_I and K_D values (P, I and D gains)
 * need to be modified to adapt to the application at hand
 */
#define K_P     0.150
#define K_I     0.15
#define K_D     0.06
struct PID_DATA pidData;

//Note: Timer1 = Clk / 8
ISR (TIMER1_COMPA_vect)
{
}

ISR (TIMER1_COMPB_vect)
{
}


//Variable that holds the counter overflows
volatile unsigned short TCNT1HH = 0;
//Capture a pulse, save its timestamp
ISR (TIMER1_CAPT_vect)
{
  int32_t time = (int32_t) TCNT1HH;
  time = time << 16;
  time |= ICR1;
  if(PIND & 0x20){
    //Is this what creates crazy oscillations? time is never negative
    time = -time;
  }
  EncLifo_Put(time);
}

//Timer1 overflow ISR 
volatile char OverflowFlag=0;
ISR (TIMER1_OVF_vect)         
{
  TCNT1HH++;  
  OverflowFlag = 1;  
}


void
ioinit (void){  
  //Setup timers
  TCCR0A = TIMER0_PWM_INIT;
  TCCR1A = 0;
  TCCR0B |= TIMER0_CLOCKSOURCE;
  TCCR1B |= TIMER1_CLOCKSOURCE;

  //Enable Timer Interrupts
  TIMSK |= _BV(ICIE1) | _BV(TOIE1);

  //Turn off PWM
  OCR0A = 0;
  
  //Set Direction Bits
  DDRB = 0x0F;
  DDRD = 0x02;

  //Set initial outputs
  PORTB = 0x01;

  //Initialize PID
  pid_Init(K_P * SCALING_FACTOR, K_I * SCALING_FACTOR , K_D * SCALING_FACTOR , &pidData);  

  //Enable Interrupts
  sei ();
}

//Get the desired speed for the wheels
volatile short reference = 0;
int16_t Get_Reference(void)
{
  return reference;
  //return -200;
}

//Get the measured speed of the wheels
int16_t Get_Measurement(void)
{
  int32_t diff;
  diff = EncLifo_Diff();
  return 1000000 / diff;
}

//Set PWM (physical input to the system)
volatile short input;
void Set_Input(int16_t inputValue)
{
  input = inputValue;
  if(inputValue < 0){
    inputValue = -inputValue;
    PORTB |= 0x01;
    PORTB &= ~0x02;
  } else {
    PORTB |= 0x02;
    PORTB &= ~0x01;
  }
  if(inputValue > 255){OCR0A=255;}
  else {OCR0A = (char)(inputValue);}

}



int main (void){
  int16_t referenceValue, measurementValue, inputValue;
  char outcount=0;
  UART_Init(38400);
  ioinit ();

  while (1){              

    
    //At every timer overflow, run PID Loop
    if(OverflowFlag){
      referenceValue = Get_Reference();
      measurementValue = Get_Measurement();
      inputValue = pid_Controller(referenceValue, measurementValue, &pidData);
      Set_Input(inputValue);
      OverflowFlag = 0;
      
      outcount++;
      if(outcount>2){
	outcount = 0;
	/*
	
	char in = UART_InChar2();
	char sign;
	char* r;
	switch(in){
	  //Set reference
	case 'v':
	  UART_OutChar('k');
	  sign = UART_InChar();
	  r = (char*) &reference;
	  r[1] = UART_InChar();
	  r[0] = UART_InChar();
	  if(sign=='-'){reference = -reference;}
	  break;
	}
	
	*/
	//Print velocity
	UART_OutChar('v');
	short measurement = Get_Measurement();
	if (measurement < 0) {
	  measurement = -measurement;
	  UART_OutChar('-');
	} else {
	  UART_OutChar('+');
	}
	char* m = (char*) &measurement;
	UART_OutChar(m[1]);
	UART_OutChar(m[0]);
	
	
	
	//Print PWM value
	UART_OutChar('p');
	char* p = (char*) &OCR0A;
	if(PINB & 0x01){
	  UART_OutChar('-');
	} else {
	  UART_OutChar('+');
	}
	UART_OutChar(p[1]);
	UART_OutChar(p[0]);      
	UART_OutChar(CR);
      }
    }
}

  return (0);
}
